import math
import numpy as np
"""
    实用性计算方法

"""

def degreeBetweenTwoPoints(x1, y1, x2, y2):
    """
    (x1, y1) 到 (x2, y2) 的角度
    :param x1:
    :param y1:
    :param x2:
    :param y2:
    :return: 0-360，x轴正方向为0度，逆时针增加角度
    """
    tmp = math.atan2(y2 - y1, x2 - x1) * 180 / math.pi
    if tmp < 0:
        tmp = tmp + 360
    return tmp


def distanceBetweenTwoPoints(x1, y1, x2, y2):
    """
    (x1, y1) 到 (x2, y2) 的距离
    :param x1:
    :param y1:
    :param x2:
    :param y2:
    :return: real distance
    """
    tmp = (x2-x1)**2 + (y2-y1)**2
    math.sqrt(tmp)
    return tmp



def TDMA(paraM: np.ndarray, vM: np.ndarray):
    """
    使用高斯消元法解 三对角矩阵
    :param paraM: 参数矩阵
    :param vM:  值矩阵
    :return np.ndarray :    解矩阵
    """
    # print(paraM.shape)
    length = paraM.shape[0]  # 方阵长度
    # 分离参数a，b,c
    a = np.zeros(length)
    a[length-1] = paraM[length-1,length-2]
    b = np.zeros(length)
    b[0] = paraM[0, 0]
    b[length-1] = paraM[length-1, length-1]
    c = np.zeros(length)
    c[0] = paraM[0, 1]
    for i in range(1, length - 1):
        a[i] = paraM[i, i - 1]
        b[i] = paraM[i, i]
        c[i] = paraM[i, i + 1]
    # 计算
    x = vM.copy()   # 返回结果
    c[0] = c[0] / b[0]
    x[0] = x[0] / b[0]
    for i in range(1, length):
        m = 1.0 / (b[i] - a[i] * c[i - 1])
        c[i] = c[i] * m
        x[i] = (x[i] - a[i] * x[i - 1]) * m

    for i in range(length - 2, -1 , -1):
        x[i] = x[i] - c[i] * x[i + 1]
    return x

